/* FlightControl

Open Source Model/High Power Rocket Flight Control System with GPS

  This software is free; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This software is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.
  
  This software is designed to run in a hardware environment compliant with
  the Open Source Flight Control Project hardware definitions.

  Use of this, or any, software - which you DOWNLOADED FROM THE INTERNET
  WITHOUT ANY WARRANTY - in any kind of self-propelled, autonomous,
  semi-autonomous, or remote controlled device, including a model or high
  power rocket, should be cause for concern, a reason to question your
  sanity, and a general admission that you, personally, accept all of
  the subsequent concequences of this action.

  You should have received a copy of the GNU Lesser General Public
  License along with this software; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

*/

#include <avr/pgmspace.h>

#include <Wire.h>
//#include <BMA180.h>
//#include <ACCEL_v1.h>  // Std Accelerometer interface - needed if passing in method
#include <BMP085.h>
#include <Servo.h>
#include <EEPROM.h>
#include <tinyOSC.h>
#include <I2Cgps.h>
#include <tinyI2Clog.h>

class BMA180
{
  public:
    BMA180(void);
    void get_accel(void);                       // of raw accelerometer data
    int x_accel;
    int y_accel;                   // The raw accelerometer readings
    int z_accel;
    int scale_num;                 // The accelerometer scale factor
    int scale_den;                 // Def: A(cm/s^2) = raw * num * 2^exp / den
    int scale_exp;                 //                = ((raw * num) << exp) / den
};

BMA180::BMA180(void) {
      x_accel = 0;
      y_accel = 0;
      z_accel = 0;
      scale_num = 1;
      scale_den = 1;
      scale_exp = 0;
    }
    
void BMA180::get_accel(void){
      // The get_accel() method loads a coherent set
}


BMA180 bma180;
BMP085 pAlt;
tinyOSC osc;

I2Cgps gps;
tinyI2Clog sd;

String fString = "";

const char comma = ',';
const char slash = '/';
const char ack_str = 'K';
char ack[] = "Kk";
char nak[] = "Nn";
char dep_str[] = "ej";
char main_str[] = "ma";
char stg_str[] = "sg";
char test_str[] = "te";
char sts_str[] = "st";
char data_str[] = "da";
char acc_oscStr[] = "ac";
char file_str[] = "fi";
char flight_str[] = "fd";
char set_str[] = "se";
char gps_str[] = "gp";
const char lst_str = 'l';
const char dat_str = 'd';
const char rec_str = 'r';
const byte FF = 0xFF;
const byte lf = 10;

Servo chute_servo;  // create servo object to control main chute deploy

const byte servoPin = 9;
const byte fet1Pin = 2;
const byte fet2Pin = 3;
const byte ledPin = 5;

/*********************************************************************
*********************************************************************
/ System Constants
***********************************************************************/
#define CHUTE_CLOSED 30
#define CHUTE_OPEN 65

#define SW_FET1 0x80
#define SW_FET2 0x20
#define SW_LED  0x08

// Need to change FET command - 2 & 3 are interrupts
// Will use 2 for I2C/UART interrupt
// 3 to jumper accel/pressure/user
#define FET1_MON 1
#define FET2_MON 0

#define EE_DPLY 16
#define EE_LNCH 18
#define EE_HSTG	20
#define EE_VSTG 22
#define EE_2STG 24
#define EE_DEP1 25
#define EE_DEP2 26
#define EE_SITE 27
#define EE_FILE 28

#define TWO_MINS 120000

/* ***************************************************************
Strings - Stored in Program Memory to save RAM
*************************************************************** */
//prog_char example_str[] PROGMEM = "example";


/* ********************************************************************************
** BMP085 Globals
******************************************************************************** */
long p = 0;         // pressure in Pa
long p_int = 0;    // Filtered pressure reading, in Pa << 10
long v_int = 0;    // Filtered pressure velocity in (Pa << 10) / (time step)
long err = 0;
int v_a;
long p_min = 0;    // Minimum pressure during recording
long v_stopped = 100;  // maximum pressure velocity for "decent"
long p_deploy = 0;
long p_launch = 0;
long p_stage = 0;
long pv_stage = 0;
long p_site = 0;
/* ********************************************************************************
** System Globals
******************************************************************************** */
// Mode Values
const byte start_mode = 1;
const byte calibrate = 2;
const byte pre_launch = 3;
const byte launch_lock = 4;
const byte flight = 5;
const byte deployed = 6;
const byte main_deployed = 7;
const byte post_flight = 8;

const int one_g = 1024;
const int half_g = 512;
const int g_limit = 100;
const int gsq_limit = 100;
const int g_launch = 3000;
const int g_stage = 100;
const int continuity = 550;  // fet continuity threshold value (c. 8.3 V) 
const int fet_scale1 = 15;
const int fet_scale2 = 7;

byte mode = 0;
int servo_cmd = 0;     // Servo command
int fet1_monitor = 0;  // FET 1 Continuity monitor A/D
int fet2_monitor = 0;  // FET 2 Continuity monitor A/D

int h_deploy = 0;   // Main Deployment altitude, in feet
int h_launch = 0;   // Minimum Altitude, in feet, for "go at launch"
int h_stage = 0;    // Minimum Altitude for "Go to stage"
int v_stage = 0;    // Minimum Velocity @h_stage, in feet/s, for "go"
int site_alt = 0;   // Site Altitude, in feet

boolean do_stage = false;
boolean stage_clear = false;
boolean do_deploy = false;
boolean do_main = false;
boolean do_record = false;
boolean file_open = false;
boolean test_mode = false;
boolean arm_mode = false;
boolean fet1_on = false;
boolean fet2_on = false;
boolean led_on = false;
boolean fet_go = false;
boolean chute_open = false;
boolean launch_hold = false;
boolean pHysteresis = false;

unsigned long cal_time = 0;
unsigned long pre_time = 0;
unsigned long launch_time = 0;
unsigned long flight_time = 0;
unsigned long stage_time = 0;
unsigned long deploy1_time = 0;
unsigned long deploy2_time = 0;
unsigned long fet1_timeout = 0;
unsigned long fet2_timeout = 0;
int fet_time = 2000;  // Maximum FET "on" time 
int flight_timeout = 2000;
int gps_valid_age = 5000;


/* ***************************************************************
Timing (tiks) Initialization
*************************************************************** */

const int tik_time = 20;    // ms betweek tik1's
const int ctrT1T2 = 5;             // number of tik1's per tik2
const int ctrT1T3 = 50;            // number of tik1's per tik3

boolean tik1 = false;
boolean tik2 = false;
boolean tik3 = false;
boolean gOn = false;
unsigned long time_now = 0;    // time in micros
unsigned long last_time = 0;   // last time in micros
unsigned long nextTik = 0;     // millis of next tik1
int tik2ctr = 0;
int tik3ctr = 0;
int sec_ctr = 0;              // total number of seconds (tik3's)

/* ***************************************************************
******************************************************************
Free Memory Function
******************************************************************
****************************************************************

extern void *__bss_end;
extern void *__brkval;
int get_free_memory()
{
  int free_memory;

  if((int)__brkval == 0)
    free_memory = ((int)&free_memory) - ((int)&__bss_end);
  else
    free_memory = ((int)&free_memory) - ((int)__brkval);

  return free_memory;
}
*/
/* ***************************************************************
******************************************************************
Print PROGMEM String Functions
******************************************************************
***************************************************************
void str_print(prog_char* str)
{
  char buf;
  int ctr = 0;
  boolean done = false;
  
  while (!done) {
    buf =  pgm_read_byte_near(str + ctr++);
    if (buf == 0) {
      done = true;
    }
    else {
      Serial.print(buf);
    }
  }
}

void str_println(prog_char* str)
{
  str_print(str);
  Serial.println();
} 

*************************************************************** */

int read_EEPROM(int addr) {
  union t {
    byte b[2];
    int i;
  } n;
  n.b[1] = EEPROM.read(addr);
  n.b[0] = EEPROM.read(++addr);
  return n.i;
}

void write_EEPROM(int addr, int data) {
  EEPROM.write(addr, highByte(data));
  EEPROM.write(++addr, lowByte(data));
}

/* ***************************************************************
******************************************************************
****************** MAIN ROUTINES SECTION  BELOW ******************
******************************************************************
*************************************************************** */

void setup() {
  
  // Initialize the digital output pins
  pinMode(fet1Pin, OUTPUT);
  digitalWrite(fet1Pin, LOW);
  pinMode(fet2Pin, OUTPUT);
  digitalWrite(fet2Pin, LOW);
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, LOW);
  
  // Initialize servo control
  chute_servo.attach(servoPin);  // attaches the servo object to servoPin (9)
  chute_servo.write(CHUTE_CLOSED);  // set servo to closed position
  chute_open = false;
  
  h_deploy = read_EEPROM(EE_DPLY);
  h_launch = read_EEPROM(EE_LNCH);
  h_stage = read_EEPROM(EE_HSTG);
  v_stage = read_EEPROM(EE_VSTG);
  site_alt = read_EEPROM(EE_SITE);
  
  do_stage = EEPROM.read(EE_2STG);
  do_deploy = EEPROM.read(EE_DEP1);
  do_main = EEPROM.read(EE_DEP2);
  
  // join i2c bus (address optional for master)
  Wire.begin();
  
  // Initialize pressure sensor & filter
  pAlt.begin();
  pAlt.start_pressure();
  delay(50);
  p_int = pAlt.pressure() << 10;
  pAlt.start_pressure();
  
  sd.begin();
  
  // Initialize serial communication 
  Serial.begin(38400);
  // Initialize OSC object, terminate string with LF
  osc.begin(Serial, lf);

  gps.begin();
  gps.setAutoLog(true);
  
  mode = start_mode;
  
}  

/* ***************************************************************
************************** loop **********************************
*************************************************************** */

void loop() {
  
  /* ***************************************************************
  Timing (tiks) Initialization
  *************************************************************** */
  
  if (millis() >= nextTik) {
    tik1 = true;
    nextTik += tik_time;
    if (++tik2ctr >= ctrT1T2) {
     tik2 = true;
     tik2ctr = 0;
     }
    if (++tik3ctr >= ctrT1T3) {
      tik3 = true;
      sec_ctr++;
      tik3ctr = 0;
    }
  }
  
    /* ***************************************************************
    tik 1 - 50 Hz
    Update the inputs & controls, update filters
    *************************************************************** */

  if (tik1) {
    tik1 = false;
    time_now = micros();    // Get a fresh time before we do the A/D's
    
    // Read acceleration and pressure
    bma180.get_accel();
    p= pAlt.pressure();
    // Get new temp reading once a second
    if(tik3) {
      pAlt.get_temp();
    }
    // restart pressure for next time
    pAlt.start_pressure();
    
    // filter pressure data
    err = p - (p_int >> 10);
    v_int += err;
    if ((mode > calibrate) && (mode < deployed)) {  // acceleration feed-forward
      v_a = 1024 - bma180.x_accel;  // or whatever axis is "+up"
      v_int += (v_a >> 5) + (v_a >> 7);
    }
    p_int += v_int + (err << 4);
    
    
    // Read FET/Continuity monitor voltages
    fet1_monitor = analogRead(FET1_MON);
    fet2_monitor = analogRead(FET2_MON);
    
    if ((!do_stage || (fet2_monitor > continuity)) && (!do_deploy || (fet1_monitor > continuity))) {
      fet_go = true;
    }
    else {
      fet_go = false;
    }
    
    // Turn off FETs after timeout
    if (fet1_on) {
      if (millis() > fet1_timeout) {
        digitalWrite(fet1Pin, LOW);
        fet1_on = false;
      }
    }
    if (fet2_on) {
      if (millis() > fet2_timeout) {
        digitalWrite(fet2Pin, LOW);
        fet2_on = false;
      }
    }
    
    // Mode control
    switch (mode) {
      case start_mode: {
        digitalWrite(ledPin, HIGH);
        boolean quiet = ((abs(bma180.y_accel) < g_limit) && (abs(bma180.z_accel) < g_limit));
        if (quiet && fet_go && !launch_hold) {
          if (pHysteresis) {
            if ((millis() - pre_time) > flight_timeout) {
              mode = calibrate;
              pHysteresis = false;
            }
          } else {
            pHysteresis = true;
            pre_time = millis();
          }
        } else {
          pHysteresis = false;
        }
        break;
      }
      case calibrate: {
        digitalWrite(ledPin, LOW);
        p_site = p_int;
        p_deploy = p_int - (600 + (p_int >> 15)) * h_deploy;
        p_launch = p_int - (600 + (p_int >> 15)) * h_launch;
        p_stage = p_int - (600 + (p_int >> 15)) * h_stage;
        pv_stage = ((-(600 + (p_int >> 15)) * v_stage) * tik_time) >> 10;
        
        cal_time = millis();
        mode = pre_launch;
        if (!do_record) {
          if (!file_open) {
            sd.open();
            file_open = true;
          }
          do_record = true;
          sd.println(fString);
        }
        String s = "";
        settingsStr(&s);
        sd.println(s);
        break;
      }
      case pre_launch:{
         if ((sec_ctr % 2) == 1) {
          digitalWrite(ledPin, LOW);
        }
        else {
          digitalWrite(ledPin, HIGH);
        }
        if (bma180.x_accel > g_launch) {
          mode = launch_lock;
          launch_time = millis();
          flight_time = launch_time + flight_timeout;
        }
        boolean quiet = ((abs(bma180.y_accel) < g_limit) && (abs(bma180.z_accel) < g_limit));
        if (!quiet) {
          if (pHysteresis) {
            if ((millis() - pre_time) > flight_timeout) {
              pHysteresis = false;
              do_record = false;
              mode = start_mode;
            }
          } else {
            pHysteresis = true;
            pre_time = millis();
          }
        } else {
          pHysteresis = false;
        }
        if (!fet_go) {
          mode = start_mode;
          do_record = false;
        }
        break;
      }
      case launch_lock: {
        if ((tik3ctr % 10) > 5) {
          digitalWrite(ledPin, LOW);
        }
        else {
          digitalWrite(ledPin, HIGH);
        }
        if (p_int < p_launch) {
          mode = flight;
          flight_time = millis();
        }
        else if (millis() > flight_time) {
          mode = pre_launch;
          flight_time = 0;
        }
        break;
      }
      case flight: {
        if ((tik3ctr % 10) > 5) {
          digitalWrite(ledPin, LOW);
        }
        else {
          digitalWrite(ledPin, HIGH);
        }
        if ((p_int < p_stage) && (v_int < pv_stage)) {
          stage_clear = true;
        }
        else {
          stage_clear = false;
        }
        if (stage_clear && (bma180.x_accel < g_stage)) {
          stage_time = millis();
          if (do_stage) {
            do_stage = false;
            fet2_on = true;
            digitalWrite(fet2Pin, HIGH);
          }
        }
        if (v_int > 0) {
          mode = deployed;
          deploy1_time = millis();
          if (do_deploy) {
            fet1_on = true;
            digitalWrite(fet1Pin, HIGH);
          }
        }
        break;
      }
      case deployed: {
        if ((tik3ctr % 10) > 5) {
          digitalWrite(ledPin, LOW);
        }
        else {
          digitalWrite(ledPin, HIGH);
        }
        if (p_int > p_deploy) {
          chute_servo.write(CHUTE_OPEN);
          chute_open = true;
          mode = main_deployed;
          deploy2_time = millis();
        }
        break;
      }
      case main_deployed: {
        if ((tik3ctr % 10) > 5) {
          digitalWrite(ledPin, LOW);
        }
        else {
          digitalWrite(ledPin, HIGH);
        }
        if (v_int < v_stopped ) {
          mode = post_flight;
          do_record = false;
          String s = "";
          flightStr(&s);
          sd.println(s);
          sd.close();
          file_open = false;
        }
        break;
      }
      case post_flight: {
        if (tik3ctr % 10 > 5) {
          digitalWrite(ledPin, LOW);
        }
        else {
          digitalWrite(ledPin, HIGH);
        }
        break;
      }
    }
  }
  
  // ***********************************************
  // The tik2 section - Executes every 100 ms (0.1 second)
  if (tik2) {
    tik2 = false;
    if (do_record) {
      String s = "";
      statusStr(&s);
      sd.println(s);
      sensorStr(&s);
      sd.println(s);
    }
  }
  
  // ***********************************************
  // The tik3 section - Executes every second
  if (tik3) {
    tik3 = false;
    gps.get_position();
    String s = "";
    statusStr(&s);
    osc.sendString(sts_str, s);
    gpsString(&s);
    osc.sendString(gps_str, s);
  }
  // ********************************************************************
  // Process Serial Terminal Inputs
  // But only if we're not in a flight mode!
  // Note: We're always sending status & GPS data to the terminal
  // ********************************************************************
  if (osc.available()) {
    if ((mode < launch_lock) || (mode > deployed)) {
      char cmdChar = osc.method[0];
      processCmd(cmdChar);
    }
  }
}

